#include "StdAfx.h"
#include "OffGridLinks.h"

#include "../NavigationSystem/NavigationSystem.h"
#include "Tile.h"

void MNM::OffMeshNavigation::TileLinks::CopyLinks(TriangleLink* links, uint16 linkCount)
{
	if (triangleLinkCount != linkCount)
	{
		SAFE_DELETE_ARRAY(triangleLinks);

		triangleLinkCount = linkCount;

		if (linkCount)
			triangleLinks = new TriangleLink[linkCount];
	}

	if (linkCount)
		memcpy(triangleLinks, links, sizeof(TriangleLink) * linkCount);
}

MNM::OffMeshNavigation::OffMeshNavigation()
: m_linkIDGenerator(0)
{

}

void MNM::OffMeshNavigation::AddLink( NavigationMesh& navigationMesh, const TriangleID startTriangleID, const TriangleID endTriangleID , const MNM::OffMeshLink& linkData )
{
	//////////////////////////////////////////////////////////////////////////
	/// Figure out the tile to operate
	MNM::TileID tileID = MNM::ComputeTileID(startTriangleID);
	
	TileLinks* pTileLinks = NULL;

	TTilesLinks::iterator tileLinksIt = m_tilesLinks.find(tileID);
	if (tileLinksIt != m_tilesLinks.end())
	{
		pTileLinks = &(tileLinksIt->second);
	}
	else
	{
		std::pair<TTilesLinks::iterator, bool> newTileIt = m_tilesLinks.insert(TTilesLinks::value_type(tileID, TileLinks()));
		assert(newTileIt.second);

		pTileLinks = &(newTileIt.first->second);
	}

	assert(pTileLinks);

	//////////////////////////////////////////////////////////////////////////
	// Find if we have entries for this triangle, if not insert at the end
	uint16 targetIdx = 0xFFFF;
	for (uint16 triIdx = 0; triIdx < pTileLinks->triangleLinkCount; ++triIdx)
	{
		if (pTileLinks->triangleLinks[triIdx].startTriangleID == startTriangleID)
		{
			targetIdx = triIdx;
			break;
		}
	}
	
	const int maxTileLinks = 1024;
	TriangleLink tempTriangleLinks[maxTileLinks];

	assert ((pTileLinks->triangleLinkCount + 1) < maxTileLinks);

	if (targetIdx != 0xFFFF)
	{
		if (targetIdx)
			memcpy(tempTriangleLinks, pTileLinks->triangleLinks, sizeof(TriangleLink) * targetIdx);

		tempTriangleLinks[targetIdx].startTriangleID = startTriangleID;
		tempTriangleLinks[targetIdx].endTriangleID = endTriangleID;
		tempTriangleLinks[targetIdx].linkID = ++m_linkIDGenerator;
		
		const int diffCount = pTileLinks->triangleLinkCount - targetIdx;
		if (diffCount)
			memcpy(&tempTriangleLinks[targetIdx+1], &pTileLinks->triangleLinks[targetIdx], sizeof(TriangleLink) * diffCount);

		pTileLinks->CopyLinks(tempTriangleLinks, pTileLinks->triangleLinkCount + 1);

		// Update links for next triangles, since we shifted the indices with the memcpy
		TriangleID currentTriangleID = startTriangleID;
		for(uint16 triIdx = targetIdx + 1; triIdx < pTileLinks->triangleLinkCount; ++triIdx)
		{
			if (pTileLinks->triangleLinks[triIdx].startTriangleID == currentTriangleID)
				continue;

			currentTriangleID = pTileLinks->triangleLinks[triIdx].startTriangleID;
			navigationMesh.grid.UpdateOffMeshLinkForTile(tileID, currentTriangleID, triIdx);
		}
	}
	else
	{
		if (pTileLinks->triangleLinkCount)
			memcpy(tempTriangleLinks, pTileLinks->triangleLinks, sizeof(TriangleLink) * pTileLinks->triangleLinkCount);
		
		tempTriangleLinks[pTileLinks->triangleLinkCount].startTriangleID = startTriangleID;
		tempTriangleLinks[pTileLinks->triangleLinkCount].endTriangleID = endTriangleID;
		tempTriangleLinks[pTileLinks->triangleLinkCount].linkID = ++m_linkIDGenerator;

		pTileLinks->CopyLinks(tempTriangleLinks, pTileLinks->triangleLinkCount + 1);

		navigationMesh.grid.AddOffMeshLinkToTile(tileID, startTriangleID, pTileLinks->triangleLinkCount - 1);
	}

	m_offmeshObjectLinks.insert(TOffMeshObjectLinks::value_type(m_linkIDGenerator, OffMeshLinkPtr(linkData.Clone())));
}

void MNM::OffMeshNavigation::RemoveLink( NavigationMesh& navigationMesh, const TriangleID boundTriangleID, const EntityId objectId )
{
	MNM::TileID tileID = ComputeTileID(boundTriangleID);

	TTilesLinks::iterator tileLinksIt = m_tilesLinks.find(tileID);

	if (tileLinksIt != m_tilesLinks.end())
	{
		const int maxTileLinks = 1024;
		TriangleLink tempTriangleLinks[maxTileLinks];

		TileLinks& tileLinks = tileLinksIt->second;

		//Note: Should be ok to copy this way, we are not going to have many links per tile...
		uint16 copyCount = 0;
		for(uint16 triIdx = 0; triIdx < tileLinks.triangleLinkCount; ++triIdx)
		{
			TriangleLink& triangleLink = tileLinks.triangleLinks[triIdx];

			if((triangleLink.startTriangleID != boundTriangleID) || !IsLinkBoundToObject(triangleLink.linkID, objectId))
			{
				tempTriangleLinks[copyCount] = triangleLink;
				copyCount++;
			}
			else
			{
				RemoveObjectLink(triangleLink.linkID);
			}
		}

		tileLinks.CopyLinks(tempTriangleLinks, copyCount);

		//Update triangle off-mesh indices
		uint16 boundTriangleLeftLinks = 0;
		TriangleID currentTriangleID(0);
		for(uint16 triIdx = 0; triIdx < tileLinks.triangleLinkCount; ++triIdx)
		{
			TriangleLink& triangleLink = tileLinks.triangleLinks[triIdx];

			if (currentTriangleID != triangleLink.startTriangleID)
			{
				currentTriangleID = triangleLink.startTriangleID;
				navigationMesh.grid.UpdateOffMeshLinkForTile(tileID, currentTriangleID, triIdx);
			}
			boundTriangleLeftLinks += (currentTriangleID == boundTriangleID) ? 1 : 0;
		}

		if (!boundTriangleLeftLinks)
		{
			navigationMesh.grid.RemoveOffMeshLinkFromTile(tileID, boundTriangleID);
		}
	}
}

void MNM::OffMeshNavigation::InvalidateLinks(const TileID tileID)
{
	TTilesLinks::iterator tileIt = m_tilesLinks.find(tileID);

	if (tileIt != m_tilesLinks.end())
	{
		const TileLinks& tileLinks = tileIt->second;
		for (uint16 triangleIdx = 0; triangleIdx < tileLinks.triangleLinkCount; ++triangleIdx)
		{
			RemoveObjectLink(tileLinks.triangleLinks[triangleIdx].linkID);
		}

		m_tilesLinks.erase(tileIt);
	}
}

MNM::OffMeshNavigation::QueryLinksResult MNM::OffMeshNavigation::GetLinksForTriangle( const TriangleID triangleID, const uint16 index ) const
{
	const MNM::TileID& tileID = ComputeTileID(triangleID);
	
	TTilesLinks::const_iterator tileCit = m_tilesLinks.find(tileID);

	if (tileCit != m_tilesLinks.end())
	{
		const TileLinks& tileLinks = tileCit->second;
		if ( index < tileLinks.triangleLinkCount )
		{
			TriangleLink* pFirstTriangle = &tileLinks.triangleLinks[index];
			uint16 linkCount = 1;

			for (uint16 triIdx = index + 1; triIdx < tileLinks.triangleLinkCount; ++triIdx)
			{
				if (tileLinks.triangleLinks[triIdx].startTriangleID == triangleID)
				{
					linkCount++;
				}
				else
				{
					break;
				}
			}

			return QueryLinksResult(pFirstTriangle, linkCount);
		}

	}

	return QueryLinksResult(NULL, 0);
}

const MNM::OffMeshLink* MNM::OffMeshNavigation::GetObjectLinkInfo(const OffMeshLinkID linkID) const
{
	TOffMeshObjectLinks::const_iterator linkIt = m_offmeshObjectLinks.find(linkID);

	return (linkIt != m_offmeshObjectLinks.end()) ? linkIt->second.get() : NULL;
}

MNM::OffMeshLink* MNM::OffMeshNavigation::GetObjectLinkInfo(const OffMeshLinkID linkID)
{
	TOffMeshObjectLinks::iterator linkIt = m_offmeshObjectLinks.find(linkID);

	return (linkIt != m_offmeshObjectLinks.end()) ? linkIt->second.get() : NULL;
}

bool MNM::OffMeshNavigation::CanUseLink( const IAIPathAgent *pRequester, const OffMeshLinkID linkID ) const
{
	const OffMeshLink* pOffMeshLink = GetObjectLinkInfo(linkID);
	if (pOffMeshLink && pRequester)
	{
		float costMultiplier = 1.0f;
		return gAIEnv.pSmartObjectManager->GetSmartObjectLinkCostFactorForMNM(*pOffMeshLink, *pRequester, &costMultiplier);
	}

	return false;
}

bool MNM::OffMeshNavigation::IsLinkBoundToObject( const OffMeshLinkID linkID, const EntityId objectId ) const
{
	TOffMeshObjectLinks::const_iterator it = m_offmeshObjectLinks.find(linkID);
	if(it != m_offmeshObjectLinks.end())
	{
		return (it->second->GetEntityId() == objectId);
	}

	return false;
}

void MNM::OffMeshNavigation::RemoveObjectLink( const OffMeshLinkID linkID )
{
	TOffMeshObjectLinks::iterator it = m_offmeshObjectLinks.find(linkID);
	if(it != m_offmeshObjectLinks.end())
	{
		m_offmeshObjectLinks.erase(it);
	}
}

#if DEBUG_MNM_ENABLED

MNM::OffMeshNavigation::ProfileMemoryStats MNM::OffMeshNavigation::GetMemoryStats( ICrySizer* pSizer ) const
{
	ProfileMemoryStats memoryStats;

	// Tile links memory
	for (TTilesLinks::const_iterator tileIt = m_tilesLinks.begin(); tileIt != m_tilesLinks.end(); ++tileIt)
	{
		memoryStats.offMeshTileLinksMemory += tileIt->second.triangleLinkCount * sizeof(TriangleLink);
	}

	size_t previousSize = pSizer->GetTotalSize();
	pSizer->AddHashMap(m_tilesLinks);
	
	memoryStats.offMeshTileLinksMemory += (pSizer->GetTotalSize() - previousSize);

	//Smart object info
	previousSize = pSizer->GetTotalSize();
	pSizer->AddHashMap(m_offmeshObjectLinks);

	memoryStats.smartObjectInfoMemory += (pSizer->GetTotalSize() - previousSize);

	//Object size
	previousSize = pSizer->GetTotalSize(); 
	pSizer->AddObjectSize(this);

	memoryStats.totalSize = memoryStats.offMeshTileLinksMemory + memoryStats.smartObjectInfoMemory + (pSizer->GetTotalSize() - previousSize);

	return memoryStats;
}

#endif
